#!/usr/bin/env python
# -*- coding:utf-8 -*-
# Ros libraries
import roslib
import rospy

import detector

# Ros Messages
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import Pose

from imagetran.msg import Calibrationimage

from probot_vision.srv import *

import cv2
from cv_bridge import CvBridge, CvBridgeError




def handle_object_detect(req):
    rospy.loginfo("Try to detect objects...")

    redObjList    = []
    greenObjList = []
    blueObjList   = []
    blackObjList  = []
    res           = DetectObjectSrvResponse.NOT_DETECTED

    image_params = rospy.get_param("~image")

    # detect colors
    try:
        imageData = rospy.wait_for_message('/image_converter/output_video', Calibrationimage, timeout=5)
        print("receive")
        print(imageData.hmatrix)
        try:
            cv_image = CvBridge().imgmsg_to_cv2(imageData.RosImage,"bgr8")

        except CvBridgeError as e:
            print(e)

        #cv2.waitKey(300)
        #cv2.imshow("test image", cv_image)
        #print("sdone")
        #cv2.waitKey(300)
        #print("zdone")
        #cv2.destroyAllWindows()

        #print("done")


        if req.objectType is DetectObjectSrvRequest.RED_OBJECT:
            red_params = rospy.get_param("~redObj")
            res, redObjList = detector.myexecute(cv_image, DetectObjectSrvRequest.RED_OBJECT, red_params, imageData.hmatrix)
            rospy.loginfo("Detect red object over, result: " + str(res))
        elif req.objectType is DetectObjectSrvRequest.GREEN_OBJECT:
            green_params = rospy.get_param("~greenObj")
            res, greenObjList = detector.myexecute(cv_image, DetectObjectSrvRequest.GREEN_OBJECT, green_params, imageData.hmatrix)
            rospy.loginfo("Detect green object over, result: " + str(res))
        elif req.objectType is DetectObjectSrvRequest.BLUE_OBJECT:
            blue_params = rospy.get_param("~blueObj")
            res, blueObjList = detector.myexecute(cv_image, DetectObjectSrvRequest.BLUE_OBJECT, blue_params, imageData.hmatrix)
            rospy.loginfo("Detect blue object over, result: " + str(res))
        elif req.objectType is DetectObjectSrvRequest.BLACK_OBJECT:
            black_params = rospy.get_param("~blackObj")
            res, blackObjList = detector.myexecute(cv_image, DetectObjectSrvRequest.BLACK_OBJECT, black_params, imageData.hmatrix)
            rospy.loginfo("Detect black object over, result: " + str(res))
        elif req.objectType is DetectObjectSrvRequest.ALL_OBJECT:
            # red
            red_params = rospy.get_param("~redObj")
            resRed, redObjList = detector.myexecute(cv_image, DetectObjectSrvRequest.RED_OBJECT, red_params, imageData.hmatrix)
            rospy.loginfo("Detect red object over, result: " + str(resRed))
            # green 
            green_params = rospy.get_param("~greenObj")
            resGreen, greenObjList = detector.myexecute(cv_image, DetectObjectSrvRequest.GREEN_OBJECT, green_params, imageData.hmatrix)
            rospy.loginfo("Detect green object over, result: " + str(resGreen))
            # blue
            blue_params = rospy.get_param("~blueObj")
            resBlue, blueObjList = detector.myexecute(cv_image, DetectObjectSrvRequest.BLUE_OBJECT, blue_params, imageData.hmatrix)
            rospy.loginfo("Detect blue object over, result: " + str(resBlue))
            # black
            black_params = rospy.get_param("~blackObj")
            resBlack, blackObjList = detector.myexecute(cv_image, DetectObjectSrvRequest.BLACK_OBJECT, black_params, imageData.hmatrix)
            rospy.loginfo("Detect black object over, result: " + str(resBlack))
            
            if resRed is not DetectObjectSrvResponse.SUCCESS and \
               resGreen is not DetectObjectSrvResponse.SUCCESS and \
               resBlue is not DetectObjectSrvResponse.SUCCESS and \
               resBlack is not DetectObjectSrvResponse.SUCCESS :
                res = DetectObjectSrvResponse.NOT_DETECTED
            else:
                res = DetectObjectSrvResponse.SUCCESS

        # Not support request
        else:
            rospy.loginfo("Not support to detect object type " + str(req.objectType))
            res = DetectObjectSrvResponse.NOT_SUPPORT
    except rospy.ROSException:
        rospy.loginfo("Timeout waiting for image data.")
        res = DetectObjectSrvResponse.TIMEOUT

    return DetectObjectSrvResponse(res, redObjList, greenObjList, blueObjList, blackObjList)

if __name__ == '__main__':
    rospy.init_node('Object_Detect')
    ser = rospy.Service('probot_detect_object', DetectObjectSrv, handle_object_detect)
    print(" Server is ready to detect ")
    rospy.spin()
